#include "viobot.h"

void VioBot::init(ros::NodeHandle& nh, bool if_pritf)
{

    pub_stereo2_ctrl = nh.advertise<sunray_viobot::algo_ctrl>("/viobot/stereo2_ctrl", 2);

    // 【订阅】算法状态 from /viobot节点
    viobot_algo_status_sub = nh.subscribe("/viobot/algo_status", 1, &VioBot::viobot_algo_status_cb, this);
 
    // 【订阅】里程计数据 from /stereo2节点
    stereo2_odometry_rect_sub = nh.subscribe("/viobot/pr_loop/odometry_rect", 1000,  &VioBot::stereo2_odometry_rect_cb, this);

    node_name = ros::this_node::getName();
    cout << GREEN << node_name << " - VIOBOT init! " << TAIL << endl;
}

void VioBot::shutdown_stereo2()
{

     cout << YELLOW << "shutdown stereo2... " << TAIL << endl;
     stereo2_ctrl.algo_enable = false;
     stereo2_ctrl.algo_reboot = false;
     stereo2_ctrl.algo_reset = false;
     pub_stereo2_ctrl.publish(stereo2_ctrl);

}

bool VioBot::start_stereo2()
{

    while(ros::ok() && algo_status.algo_status != "ready") {

        cout << YELLOW << "start stereo2... " << TAIL << endl;
        stereo2_ctrl.algo_enable = true;
        stereo2_ctrl.algo_reboot = false;
        stereo2_ctrl.algo_reset = false;
        pub_stereo2_ctrl.publish(stereo2_ctrl);

        ros::spinOnce();
        sleep(1);
    }

    return 1;
}

void VioBot::viobot_algo_status_cb(const sunray_viobot::algo_status::ConstPtr &msg)
{
    algo_status = *msg;
}

void VioBot::stereo2_odometry_rect_cb(const nav_msgs::Odometry::ConstPtr &msg)
{
    odom_rect = *msg;
    cout.precision(15);
    std::cout << odom_rect.header.stamp.toSec() << endl
              << "odom(tum type): x y z qx qy qz qw" << endl
              << odom_rect.pose.pose.position.x << " "
	          << odom_rect.pose.pose.position.y << " "
	          << odom_rect.pose.pose.position.z << " "
              << odom_rect.pose.pose.orientation.x << " "
	          << odom_rect.pose.pose.orientation.y << " "
	          << odom_rect.pose.pose.orientation.z << " "
              << odom_rect.pose.pose.orientation.w << " "
              << endl << endl;
/*
    tf::Quaternion original_quat(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);

    tf::Quaternion quat_rotate;
    quat_rotate.setRPY(0, -M_PI/2, M_PI/2);
    tf::Quaternion final_quat = original_quat * quat_rotate;
    q_viobot = Eigen::Quaterniond(final_quat.w(), final_quat.x(), final_quat.y(), final_quat.z());
    euler_viobot = quaternion_to_euler_viobot(q_viobot);

    viobot_pos[0] = odom_rect.pose.pose.position.x;
    viobot_pos[1] = odom_rect.pose.pose.position.y;
    viobot_pos[2] = odom_rect.pose.pose.position.z;
    viobot_vel[0] = odom_rect.twist.twist.linear.x;
    viobot_vel[1] = odom_rect.twist.twist.linear.y;
    viobot_vel[2] = odom_rect.twist.twist.linear.z;
    viobot_yaw = euler_viobot[2];
    q_viobot_msg.x = q_viobot.x();
    q_viobot_msg.y = q_viobot.y();
    q_viobot_msg.z = q_viobot.z();
    q_viobot_msg.w = q_viobot.w();

    if(algo_status.algo_status == "stereo2_running")
    {
        viobot_state.vio_start = true;
    }else
    {
        viobot_state.vio_start = false;
    }
    viobot_state.position[0] = viobot_pos[0];
    viobot_state.position[1] = viobot_pos[1];
    viobot_state.position[2] = viobot_pos[2];
    viobot_state.velocity[0] = viobot_vel[0];
    viobot_state.velocity[1] = viobot_vel[1];
    viobot_state.velocity[2] = viobot_vel[2];
    viobot_state.attitude[0] = euler_viobot[0];
    viobot_state.attitude[1] = euler_viobot[1];
    viobot_state.attitude[2] = euler_viobot[2];
    viobot_state.attitude_q = q_viobot_msg;
    viobot_state_pub.publish(viobot_state);
*/
}

Eigen::Vector3d VioBot::quaternion_to_euler_viobot(const Eigen::Quaterniond &q) 
{
    Eigen::Matrix3d rotation_matrix = q.normalized().toRotationMatrix();

    //将Eigen::Matrix3d 手动转换为 tf::Matrix3x3
    tf::Matrix3x3 tf_rotation_matrix(rotation_matrix(0, 0), rotation_matrix(0, 1), rotation_matrix(0, 2),
                                     rotation_matrix(1, 0), rotation_matrix(1, 1), rotation_matrix(1, 2),
                                     rotation_matrix(2, 0), rotation_matrix(2, 1), rotation_matrix(2, 2));

    double yaw, pitch, roll;
    tf_rotation_matrix.getRPY(roll, pitch, yaw);

    Eigen::Vector3d euler;
    euler[0] = roll;
    euler[1] = pitch;
    euler[2] = yaw;

    return euler;
}
